%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Edited by Shu Jiang, 2011.
% ALL RIGHTS ARE RESERVED.
% mailto: shujiang@tamu.edu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate the slope from the hip to swing leg ankle
% calculate the torso slope and the linearized slope 
function ret = Knee2D_slope_V2(xpos,ypos,zpos,Lc,Lt,LT,angle,sns_brk,StanceLegIndex)
%     humanData = getData(name,i);

StartPt = sns_brk(2);
EndPt = sns_brk(3);
% from heel strike to the other heel strike
%%
for i = 1:length(xpos.lknee)
    % right leg is the stance leg
    if StanceLegIndex == 1
        % non-stance slope
        ns_slope(i) = NonStanceSlope(xpos.lhip(i),ypos.lhip(i),...
            xpos.lankle_avg(i),ypos.lankle_avg(i));

        %  Torso stance slope
        torso_sslope(i) = StanceTorsoSlope(xpos.torso(i),ypos.torso(i),...
            xpos.rankle_avg(i),ypos.rankle_avg(i));
        %  Torso non-stance slope
        torso_nsslope(i) = NonStanceTorsoSlope(xpos.torso(i),ypos.torso(i),...
            xpos.lankle_avg(i),ypos.lankle_avg(i));
    else
        % non-stance slope
        ns_slope(i) = NonStanceSlope(xpos.rhip(i),ypos.rhip(i),...
            xpos.rankle_avg(i),ypos.rankle_avg(i));
        % Torso stance slope
         torso_sslope(i) = NonStanceTorsoSlope(xpos.torso(i),ypos.torso(i),...
            xpos.lankle_avg(i),ypos.lankle_avg(i));
        % Torso non-stance slope
        torso_nsslope(i) = StanceTorsoSlope(xpos.torso(i),ypos.torso(i),...
            xpos.rankle_avg(i),ypos.rankle_avg(i));
      
    end

end
ret.nsslope = ns_slope(StartPt:EndPt);
ret.storso = torso_sslope(StartPt:EndPt);
ret.nstorso = torso_nsslope(StartPt:EndPt);

ret.Linearized_sslope = StanceSlopeLinearized(Lc,Lt,angle);
ret.Linearized_nsslope = NonStanceSlopeLinearized(Lc,Lt,angle);
ret.Linearized_storso = STorsoSlope_Linearized(Lc,Lt,LT,angle);
ret.Linearized_nstorso = NSTorsoSlope_Linearized(Lc,Lt,LT,angle);

end
% calculate the linearized stance slope
function ret = StanceSlopeLinearized(Lc,Lt,angle)
ret = -angle.sa-(Lt/(Lc+Lt)).*angle.sknee;
end 

% use the angle_avg
function ret = NonStanceSlope(xhip,yhip,xankle,yankle)
  ret =   (xhip-xankle)/(yhip-yankle);
end

% calculated the linearized non-stance slope
function ret = NonStanceSlopeLinearized(Lc,Lt,angle)
  ret = (Lc.*(angle.sa+angle.sknee+angle.theta3-angle.theta4-angle.nsknee)+...
    Lt*(angle.sa+angle.sknee+angle.theta3-angle.theta4))/(-Lc-Lt);
end

% calculate the stance torso slope
% use the average value of ankle
function ret = StanceTorsoSlope(xtorso,ytorso,xankle,yankle)
        ret = (xtorso-xankle)/(ytorso-yankle);
end
% linearized stance torso slope
function ret = STorsoSlope_Linearized(Lc,Lt,LT,angle)
     ret =  -(Lt+LT)/(Lc+Lt+LT)*angle.sknee-Lt*angle.theta3/(Lc+Lt+LT)...
 -angle.sa;
end
% linearized non-stance torso slope
function ret = NSTorsoSlope_Linearized(Lc,Lt,LT,angle)
ret = (Lc)*angle.nsknee/(Lc+Lt+LT)-angle.sa-...
    angle.sknee-angle.theta3+(Lc+Lt).*angle.theta4/(Lc+Lt+LT);
end
% calculate the non-stance torso slope
% use the average value of ankle
function ret = NonStanceTorsoSlope(xtorso,ytorso,xankle,yankle)
    ret = StanceTorsoSlope(xtorso,ytorso,xankle,yankle);
end